/* 二自由度云台机器人控制平台 
 * 作者：HonestQiao <honestqiao@163.com>
 * 版本：V1.0
*/

#include <Arduino.h>
#include <stdlib.h>

// 时间库
#include <TimeLib.h>

// WiFi库
#include <WiFi.h>
#include <WiFiMulti.h>
#include <WiFiUdp.h>
#include <Wire.h>

// MPU6050库
#include <MPU6050_tockn.h>

// Servo驱动库
#include <Adafruit_PWMServoDriver.h>

// OLED库
#include <Adafruit_SSD1306.h>
#include <U8g2_for_Adafruit_GFX.h>

// OLED定义
// 屏幕大小定义
#define SCREEN_WIDTH 128  // 宽度
#define SCREEN_HEIGHT 64  // 高度

// OLED I2C定义
#define OLED_RESET -1        // RESET
#define SCREEN_ADDRESS 0x3C  // I2C地址

// Adafruit_SSD1306显示对象
Adafruit_SSD1306 display(SCREEN_WIDTH, SCREEN_HEIGHT, &Wire, OLED_RESET);

// U8G2_FOR_ADAFRUIT_GFX显示对象
U8G2_FOR_ADAFRUIT_GFX u8g2_for_adafruit_gfx;

// OLED状态
int oled_status = 0;

// 按键
#define BUTTON 27  // 板载按键
int buttonCount = 0;
int buttonStatusPrev = LOW;  // 按键默认状态

// WiFi定义
#define WIFI_SSID "OpenBSD"
#define WIFI_PASS "13581882013"

WiFiMulti WiFiMulti;

// NTP定义
static const char ntpServerName[] = "ntp.aliyun.com";
const int timeZone = 8;  // 时区

// 通讯数据包定义
const int NTP_PACKET_SIZE = 48;
byte packetBuffer[NTP_PACKET_SIZE];

// UDP定义
WiFiUDP Udp;
unsigned int localPort = 8888;

// NTP方法和对象定义
time_t getNtpTime();

// 时间显示状态
time_t prevDisplay = 0;

// MPU6050定义
MPU6050 mpu6050(Wire);
float angleX, angleY, angleZ, gyroz;

// PCA9685舵机驱动定义
int my_14 = 90;  // 纵向，默认角度
int my_15 = 90;  // 横向，默认角度

float _min_pulse_width, _max_pulse_width;
Adafruit_PWMServoDriver pwm0x40 = Adafruit_PWMServoDriver(0x40);

// 滤波数据
double P[2][2] = { { 1, 0 }, { 0, 1 } };
double Pdot[4] = { 0, 0, 0, 0 };
static const double Q_angle = 0.001, Q_gyro = 0.003, R_angle = 0.5, dtt = 0.005, C_0 = 1;
double q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;

/** 
 * Kalman_Filter  卡尔曼滤波 
 * @angle_m: 角度
 * @gyro_m: 角速度
**/
void Kalman_Filter(double angle_m, double gyro_m) {
  angleZ += (gyro_m - q_bias) * dtt;
  Pdot[0] = Q_angle - P[0][1] - P[1][0];
  Pdot[1] = -P[1][1];
  Pdot[2] = -P[1][1];
  Pdot[3] = Q_gyro;
  P[0][0] += Pdot[0] * dtt;
  P[0][1] += Pdot[1] * dtt;
  P[1][0] += Pdot[2] * dtt;
  P[1][1] += Pdot[3] * dtt;
  angle_err = angle_m - angleZ;
  PCt_0 = C_0 * P[0][0];
  PCt_1 = C_0 * P[1][0];
  E = R_angle + C_0 * PCt_0;
  K_0 = PCt_0 / E;
  K_1 = PCt_1 / E;
  t_0 = PCt_0;
  t_1 = C_0 * P[0][1];
  P[0][0] -= K_0 * t_0;
  P[0][1] -= K_0 * t_1;
  P[1][0] -= K_1 * t_0;
  P[1][1] -= K_1 * t_1;
  angleZ += K_0 * angle_err;
  q_bias += K_1 * angle_err;
}

/** 
 * showtime  显示时间 
**/
void showtime() {
  int Hour = hour();
  int Min = minute();
  int Sec = second();
  int HourHigh, HourLow, MinHigh, MinLow, SecHigh, SecLow;

  char buff[50];
  sprintf(buff, "%02d:%02d:%02d", Hour, Min, Sec);
  // Serial.println(buff);
  u8g2_for_adafruit_gfx.setCursor(30, 12);  // start writing at this position
  u8g2_for_adafruit_gfx.print(buff);
}

/** 
 * getNtpTime  NTP对时 
**/
time_t getNtpTime() {
  IPAddress ntpServerIP;  // NTP 服务器地址

  while (Udp.parsePacket() > 0)
    ;  // 丢弃任何先前接收的数据包

  // 发起NTP请求
  Serial.println("Transmit NTP Request");
  WiFi.hostByName(ntpServerName, ntpServerIP);  // 获取IP
  Serial.print(ntpServerName);
  Serial.print(": ");
  Serial.println(ntpServerIP);

  sendNTPpacket(ntpServerIP);                   // 发送数据包

  uint32_t beginWait = millis();
  while (millis() - beginWait < 1500) {
    int size = Udp.parsePacket();
    if (size >= NTP_PACKET_SIZE) {
      Serial.println("Receive NTP Response");
      Udp.read(packetBuffer, NTP_PACKET_SIZE);  // 读取数据包
      unsigned long secsSince1900;

      // 转换数据
      secsSince1900 = (unsigned long)packetBuffer[40] << 24;
      secsSince1900 |= (unsigned long)packetBuffer[41] << 16;
      secsSince1900 |= (unsigned long)packetBuffer[42] << 8;
      secsSince1900 |= (unsigned long)packetBuffer[43];

      // 返回数据
      return secsSince1900 - 2208988800UL + timeZone * SECS_PER_HOUR;
    }
  }
  Serial.println("No NTP Response :-(");

  return 0;
}

/** 
 * sendNTPpacket  发送NTP数据包
**/
void sendNTPpacket(IPAddress &address) {
  // 清理buff
  memset(packetBuffer, 0, NTP_PACKET_SIZE);

  // 生成请求数据
  packetBuffer[0] = 0b11100011;  // LI, Version, Mode
  packetBuffer[1] = 0;           // Stratum, or type of clock
  packetBuffer[2] = 6;           // Polling Interval
  packetBuffer[3] = 0xEC;        // Peer Clock Precision
  // 8 bytes of zero for Root Delay & Root Dispersion
  packetBuffer[12] = 49;
  packetBuffer[13] = 0x4E;
  packetBuffer[14] = 49;
  packetBuffer[15] = 52;

  // 发送NTP数据包
  Udp.beginPacket(address, 123);
  Udp.write(packetBuffer, NTP_PACKET_SIZE);
  Udp.endPacket();
}

void setup() {
  // 按键初始化
  pinMode(BUTTON, INPUT_PULLUP);

  // 串口初始化
  Serial.begin(115200);

  // 通讯初始化
  Wire.begin();

  // OLED初始化
  Serial.println(F("SSD1306 init:"));
  if (!display.begin(SSD1306_SWITCHCAPVCC, SCREEN_ADDRESS)) {
    Serial.println(F("SSD1306 allocation failed"));
    for (;;)
      ;  // 循环
  } else {
    Serial.println(F("SSD1306 allocation ok"));
    oled_status = 1;
  }

  // 显示默认 Adafruit splash screen
  display.display();
  delay(1000);

  // 关联u8g8到Adafruit GFX
  u8g2_for_adafruit_gfx.begin(display);

  // 输出Booting...
  display.clearDisplay();                                       // 清除缓存
  u8g2_for_adafruit_gfx.setFont(u8g2_font_unifont_t_chinese2);  // 中文字体
  u8g2_for_adafruit_gfx.setCursor(25, 35);                      // 显示坐标
  u8g2_for_adafruit_gfx.print("Booting...");
  display.display();

  // WiFi连接处理
  WiFiMulti.addAP(WIFI_SSID, WIFI_PASS);
  while (WiFiMulti.run() != WL_CONNECTED) {
    Serial.print(".");
    delay(500);
  }

  // 显示连接状态
  display.clearDisplay();
  u8g2_for_adafruit_gfx.setFont(u8g2_font_unifont_t_chinese2);
  u8g2_for_adafruit_gfx.setCursor(10, 35);
  u8g2_for_adafruit_gfx.print("WiFi connected.");
  display.display();

  // 显示获取的IP信息
  Serial.print("IP number assigned by DHCP is ");
  Serial.println(WiFi.localIP());

  // 开启UDP
  Serial.println("Starting UDP");
  Udp.begin(localPort);
  Serial.print("Local port: ");
  Serial.println(localPort);
  Serial.println("waiting for sync");

  // 设置NTP对时
  setSyncProvider(getNtpTime);
  setSyncInterval(300);

  delay(500);

  // PCA9685初始化
  pwm0x40.begin();
  pwm0x40.setOscillatorFrequency(27000000);
  pwm0x40.setPWMFreq(50);
  _min_pulse_width = 123;
  _max_pulse_width = 490;
  pwm0x40.setPWM(15, 0, map(my_15, 0, 180, _min_pulse_width, _max_pulse_width));
  pwm0x40.setPWM(14, 0, map(my_14, 0, 180, _min_pulse_width, _max_pulse_width));

  // MPU6050初始化
  mpu6050.begin();

  //MPU6050 自校验
  mpu6050.calcGyroOffsets(true);

  // 显示MPU6050状态
  display.clearDisplay();
  u8g2_for_adafruit_gfx.setFont(u8g2_font_unifont_t_chinese2);
  u8g2_for_adafruit_gfx.setCursor(10, 35);
  u8g2_for_adafruit_gfx.print("MPU6050 init ok.");
  display.display();

  // 延时
  delay(500);
}

void loop() {
  // 清除显示缓存
  display.clearDisplay();

  // 画框
  display.drawRoundRect(0, 0, display.width() - 1, 16, 1, SSD1306_WHITE);                      // 状态栏
  display.drawRoundRect(0, 16, display.width() - 1, display.height() - 16, 1, SSD1306_WHITE);  // 内容区


  u8g2_for_adafruit_gfx.setFontDirection(0);        // 显示方向：从左到右(默认)
  u8g2_for_adafruit_gfx.setForegroundColor(WHITE);  // 应用Adafruit GFX颜色

  u8g2_for_adafruit_gfx.setFont(u8g2_font_siji_t_6x10);    // icon字体
  u8g2_for_adafruit_gfx.setFontMode(1);                    // 使用 u8g2 透明模式(默认)
  u8g2_for_adafruit_gfx.drawGlyph(0, 12, 0x0e200);         // 电源图标
  u8g2_for_adafruit_gfx.drawGlyph(128 - 16, 12, 0x0e21a);  // WiFi图标

  // 按键处理
  int buttonState = digitalRead(BUTTON);  // 获取按键状态
  if (buttonState != buttonStatusPrev) {
    buttonStatusPrev = buttonState;
    if (buttonState == LOW) {  // LOW表示按下
      buttonCount++;
    }
  }

  if (buttonCount % 2 == 0) {                              // 偶数次-运行，奇数次-停止
    u8g2_for_adafruit_gfx.drawGlyph(0 + 12, 12, 0x0e059);  // 停止
  } else {
    u8g2_for_adafruit_gfx.drawGlyph(0 + 12, 12, 0x0e058);  // 运行
  }

  // 更新MPU6050数据
  mpu6050.update();

  // 读取MPU6050数值
  angleX = mpu6050.getAngleX();
  angleZ = mpu6050.getAngleZ();
  gyroz = mpu6050.getGyroZ();

  // 滤波处理
  Kalman_Filter(angleZ, gyroz);

  // 计算角度
  int pos_x = int(angleX) + 90;
  int pos_z = int(angleZ) + 90;

  // 输出信息
  Serial.print("angleX:");
  Serial.print(angleX);
  Serial.print("\tangleZ:");
  Serial.print(angleZ);
  Serial.print("\tpos_x:");
  Serial.print(pos_x);
  Serial.print("\tpos_z:");
  Serial.println(pos_z);

  // 控制纵向舵机
  my_14 = pos_z;

  // 控制横向舵机
  my_15 = pos_x;

  if (buttonCount % 2 == 1) {
    if(my_14>=0) {
      pwm0x40.setPWM(14, 0, map(my_14, 0, 180, _min_pulse_width, _max_pulse_width));
    }

    if(my_15>=0) {
      pwm0x40.setPWM(15, 0, map(my_15, 0, 180, _min_pulse_width, _max_pulse_width));
    }
  }

  if(buttonCount==0) {
    // 显示平台名称
    u8g2_for_adafruit_gfx.setFont(u8g2_font_unifont_t_chinese2);  // 中文字体
    u8g2_for_adafruit_gfx.setFontMode(1);                         // 使用 u8g2 透明模式(默认)
    u8g2_for_adafruit_gfx.setCursor(30, 35);
    u8g2_for_adafruit_gfx.print("二自由度");
    u8g2_for_adafruit_gfx.setCursor(25, 55);
    u8g2_for_adafruit_gfx.print("机器人平台");
  } else {
    // 显示坐标
    u8g2_for_adafruit_gfx.setFont(u8g2_font_unifont_t_86);  // 特殊字体
    u8g2_for_adafruit_gfx.setFontMode(1);                         // 使用 u8g2 透明模式(默认)

    u8g2_for_adafruit_gfx.drawGlyph(25, 35, 0x02b6f);         // 纵轴
    u8g2_for_adafruit_gfx.drawGlyph(25, 55, 0x02b65);         // 横轴

    u8g2_for_adafruit_gfx.setFont(u8g2_font_unifont_t_chinese2);  // 中文字体
    u8g2_for_adafruit_gfx.setFontMode(1);                         // 使用 u8g2 透明模式(默认)
    
    u8g2_for_adafruit_gfx.setCursor(55, 35);
    u8g2_for_adafruit_gfx.print(my_14);
    u8g2_for_adafruit_gfx.setCursor(55, 55);
    u8g2_for_adafruit_gfx.print(my_15);
  }

  // 显示时间
  if (timeStatus() != timeNotSet) {  // 检查更新最新时间
    if (now() != prevDisplay) {
      prevDisplay = now();
    }
  }
  showtime();  // 显示时间

  // 显示
  display.display();

  // 延时
  delay(50);
}